#include "ros_web_bridge/draco_pcl_converter.hpp"
#include <draco/compression/encode.h>
#include <draco/compression/decode.h>
#include <iostream>

std::unique_ptr<draco::PointCloud> DracoPCLConverter::PCLToDraco(
    const pcl::PointCloud<pcl::PointXYZI>::Ptr& pcl_cloud) {
    
    auto draco_cloud = std::make_unique<draco::PointCloud>();
    
    // 设置点数量
    draco_cloud->set_num_points(pcl_cloud->size());
    
    // 添加位置属性 (XYZ)
    draco::GeometryAttribute pos_att;
    pos_att.Init(draco::GeometryAttribute::POSITION, nullptr, 3, 
                 draco::DataType::DT_FLOAT32, false, sizeof(float) * 3, 0);
    const int pos_att_id = draco_cloud->AddAttribute(pos_att, true, pcl_cloud->size());
    
    // 添加强度属性 (I)
    draco::GeometryAttribute intensity_att;
    intensity_att.Init(draco::GeometryAttribute::GENERIC, nullptr, 1, 
                       draco::DataType::DT_FLOAT32, false, sizeof(float), 0);
    const int intensity_att_id = draco_cloud->AddAttribute(intensity_att, true, pcl_cloud->size());
    
    // 填充数据
    for (size_t i = 0; i < pcl_cloud->size(); ++i) {
        const auto& point = (*pcl_cloud)[i];
        
        // 设置位置
        float pos[3] = {point.x, point.y, point.z};
        draco_cloud->attribute(pos_att_id)->SetAttributeValue(
            draco::AttributeValueIndex(i), pos);
        
        // 设置强度
        float intensity = point.intensity;
        draco_cloud->attribute(intensity_att_id)->SetAttributeValue(
            draco::AttributeValueIndex(i), &intensity);
    }
    
    return draco_cloud;
}

pcl::PointCloud<pcl::PointXYZI>::Ptr DracoPCLConverter::DracoToPCL(
    const draco::PointCloud& draco_cloud) {
    
    auto pcl_cloud = std::make_shared<pcl::PointCloud<pcl::PointXYZI>>();
    pcl_cloud->resize(draco_cloud.num_points());
    
    // 获取位置属性
    const auto* pos_att = draco_cloud.GetNamedAttribute(draco::GeometryAttribute::POSITION);
    const auto* intensity_att = draco_cloud.GetAttributeByUniqueId(1); // 假设强度属性ID为1
    
    for (draco::PointIndex i(0); i < draco_cloud.num_points(); ++i) {
        auto& point = (*pcl_cloud)[i.value()];
        
        // 获取位置
        if (pos_att) {
            float pos[3];
            pos_att->GetValue(pos_att->mapped_index(i), pos);
            point.x = pos[0];
            point.y = pos[1];
            point.z = pos[2];
        }
        
        // 获取强度
        if (intensity_att) {
            float intensity;
            intensity_att->GetValue(intensity_att->mapped_index(i), &intensity);
            point.intensity = intensity;
        } else {
            point.intensity = 0.0f;
        }
    }
    
    return pcl_cloud;
}

std::vector<uint8_t> DracoPCLConverter::CompressPointCloud(
    const pcl::PointCloud<pcl::PointXYZI>::Ptr& pcl_cloud, int compression_level) {
    
    // 转换为Draco点云
    auto draco_cloud = PCLToDraco(pcl_cloud);
    
    // 配置编码器
    draco::Encoder encoder;
    encoder.SetAttributeQuantization(draco::GeometryAttribute::POSITION, 14);
    encoder.SetAttributeQuantization(draco::GeometryAttribute::GENERIC, 12);
    encoder.SetSpeedOptions(10 - compression_level, 10 - compression_level);
    
    // 编码 - 修复类型转换问题
    draco::EncoderBuffer buffer;
    const draco::Status status = encoder.EncodePointCloudToBuffer(*draco_cloud, &buffer);
    
    if (!status.ok()) {
        std::cerr << "Error encoding point cloud: " << status.error_msg() << std::endl;
        return {};
    }
    
    // 修复：正确的类型转换
    const char* char_data = buffer.data();
    const uint8_t* uint8_data = reinterpret_cast<const uint8_t*>(char_data);
    return std::vector<uint8_t>(uint8_data, uint8_data + buffer.size());
}

pcl::PointCloud<pcl::PointXYZI>::Ptr DracoPCLConverter::DecompressPointCloud(
    const std::vector<uint8_t>& compressed_data) {
    
    // 解码 - 修复类型转换
    draco::DecoderBuffer buffer;
    const char* char_data = reinterpret_cast<const char*>(compressed_data.data());
    buffer.Init(char_data, compressed_data.size());
    
    draco::Decoder decoder;
    auto result = decoder.DecodePointCloudFromBuffer(&buffer);
    
    if (!result.ok()) {
        std::cerr << "Error decoding point cloud: " << result.status().error_msg() << std::endl;
        return nullptr;
    }
    
    // 转换回PCL
    return DracoToPCL(*result.value());
}